//
#include 	<system.h>

#include "EESD.h"
#include "cc2420.h"
#include "Hbridge.h"

#pragma DATA _CONFIG1H, _OSC_HS_1H //10 mhz
#pragma DATA _CONFIG2H, _WDT_OFF_2H 
#pragma DATA _CONFIG4L, _LVP_OFF_4L 
#pragma DATA _CONFIG3H, _MCLRE_ON_3H


#pragma CLOCK_FREQ 10000000

extern bit l_receive; //semaphore for the left chipcon receiving a packet
extern bit r_receive; //semaphore for the right chipcon receiving a packet
extern short direction;

typedef struct payload
	{
		short id;
		short type;
		short timestamp;
		short rssi;
	} Payload;

Payload SetPayloadToZero(Payload);

void main(void)
{
	// Initializes SPI and serial port
	SPI_init();
	serial_init();
	LCD_init();
	CC2420_init();
	delay_ms(200);
	intrpt_init();
	hbridge_init();

    // Defines the speeds used (scale 0-9) and the difference in dbm that
	// indicates that the beacon is in the center
	#define dbm_diff			3
	#define turn_speed			6
	#define straight_speed		6

	short counter = 0;
	short temp;
	char* packet;
	Payload l_payload, r_payload;
	bool repel_rcvd = false;
	putstring("\r\nTest program");
	
	// clears the semaphores
	l_receive = 0; 
	r_receive = 0;
	
	
	// The code block below provides a current rush to the motors
	// that is necessary to get the motors running in the very beginning
	forward(6);
	delay_ms(250);
	delay_ms(250);
	//--------------------------
	while(1)
	{
		
		
		if (l_receive == 1) // Packet received, left semanphore
		{
			
			packet = receive_pkt_l();	// Receives packet
		    l_receive = 0;				//Clears the semaphore
		    
		    l_payload.id = packet[0];
		    l_payload.type = packet[1];
		    l_payload.timestamp = packet[2];
		    l_payload.rssi = packet[3];
		}
		
		if (r_receive == 1) // Packet received, right semanphore
		{
			//brake();
		    packet = receive_pkt_r();	// Receives packet
		    r_receive = 0;				//Clears the semaphore
		    
		    r_payload.id = packet[0];
		    r_payload.type = packet[1];
		    r_payload.timestamp = packet[2];
		    r_payload.rssi = packet[3];

		}
	
		/*----------------------------------------------------
		---------- The negative sign in front of the ---------
		---------- rssi values was dropped for ---------------
		---------- simplicity's sake. Smaller rssi -----------
		---------- values are indeed stronger signals. -------
		-----------------------------------------------------*/
		
		
		if (r_payload.timestamp == l_payload.timestamp) // Check if timestamps are the same
		{
			if (r_payload.type == 0xBB && l_payload.type == 0xBB)	// Check if repel
			{
				// Check for repel threshold value
				if (r_payload.rssi <= REPEL_THRESH || l_payload.rssi <= REPEL_THRESH) 
				{
				repel_rcvd = true;
				counter = counter + 1;
					// If center repel
				    if((l_payload.rssi - r_payload.rssi) >= -dbm_diff && (l_payload.rssi - r_payload.rssi) <= dbm_diff)
					{
						putstring("\r\n      Center Repel");
						putdec(l_payload.rssi);
						putstring(" vs ");
						putdec(r_payload.rssi);
						
						if(direction!=4) reverse(straight_speed);
						
						LCD_clear();
						LCD_printf("   --->Repel<---");
					}
					
					// If right repel
					else if(l_payload.rssi < r_payload.rssi)
					{
						putstring("\r\nLeft Repel: ");
						putdec(l_payload.rssi);
						putstring(" vs ");
						putdec(r_payload.rssi);
						
						if(direction!=3) turn_left(turn_speed);
						
						LCD_clear();
						LCD_printf("           Repel--->");
						
						
					}
					// If left repel
					else if(l_payload.rssi > r_payload.rssi)
					{
						putstring("\r\n               Right Repel");
						putdec(l_payload.rssi);
						putstring(" vs ");
						putdec(r_payload.rssi);
						
						if(direction!=2) turn_right(turn_speed);
						
						LCD_clear();
						LCD_printf("<---Repel");
					}
				
					if (counter >= COUNTER_THRESH)
					{
					counter = 0;
					//move to the next waypoint beacon
					putstring("\r\nNext Beacon");
					
					LCD_setpos(5,0);
					LCD_printf("Beacon: ");					
					}
				// Set the Payload variables to zero
				l_payload = SetPayloadToZero(l_payload);
			    r_payload = SetPayloadToZero(r_payload);	
				}
				
				// If not BB type, then not repel signal
				else 
				{
				repel_rcvd = false;
				}
			
			}
			// If attract and not repel signal received
			else if ((r_payload.type == 0xAA && l_payload.type == 0xAA) && repel_rcvd == false)
			{
				// If center attract
				if ((l_payload.rssi - r_payload.rssi) >= -dbm_diff && (l_payload.rssi - r_payload.rssi) <= dbm_diff)
				{
					putstring("\r\n      Center Attract");
					putdec(l_payload.rssi);
					putstring(" vs ");
					putdec(r_payload.rssi);
					
					
					if(direction!=1) forward(straight_speed);
					
					LCD_clear();
					LCD_printf("  --->Attract<---");
				}
				
				// If right attract
				else if(l_payload.rssi < r_payload.rssi)
				{
					putstring("\r\nLeft Attract");
					putdec(l_payload.rssi);
					putstring(" vs ");
					putdec(r_payload.rssi);
					
					if(direction!=2) turn_right(turn_speed);
					
					LCD_clear();
					LCD_printf("         Attract--->");
				}
				
				// If left attract
				else if (l_payload.rssi > r_payload.rssi)
				{
				    putstring("\r\n               Right Attract");
				    putdec(l_payload.rssi);
					putstring(" vs ");
					putdec(r_payload.rssi);
					
					if(direction!=3) turn_left(turn_speed);
					
					LCD_clear();
					LCD_printf("<---Attract");
					
					
				}
			// Clear Payload variables
			l_payload = SetPayloadToZero(l_payload);
			r_payload = SetPayloadToZero(r_payload);		
			}
		}
		else		// If the timestamps are not the same an error has occured
		{
			putstring("Error!");
			
			l_payload = SetPayloadToZero(l_payload); // Resets payload variables
			r_payload = SetPayloadToZero(r_payload);
			
			l_receive = 0;				// Clears semaphores		    
		    r_receive = 0;
		    			
			CSN_r = 0;					// Sets receive state of the CC2420 and flushes
			temp =SSPsend(SXOSCON);		// the RXFIFO
			temp =SSPsend(STXCAL);
			temp =SSPsend(SRXON);
			temp =SSPsend(SFLUSHRX);
			temp =SSPsend(SFLUSHRX);
			CSN_r =1;
			
			CSN_l = 0;
			temp =SSPsend(SXOSCON);
			temp =SSPsend(STXCAL);
			temp =SSPsend(SRXON);
			temp =SSPsend(SFLUSHRX);
			temp =SSPsend(SFLUSHRX);
			CSN_l = 1;
			
			
		}
			
			
	}
}


Payload SetPayloadToZero(Payload package)
{
	package.id = 0;
	package.type = 0;
	package.timestamp = 0;
	package.rssi = 0;
	
	return package;
}
